#include <RosNodeSerial.h>


uint8_t idx = 0;  
byte *p, *sdata;    
byte incomingByte;
byte incomingBytePrev;
uint16_t bufStartFrame; 

RosNodeCommand Command;
RosNodeFeedback Feedback;
RosNodeFeedback NewFeedback;

#if SERIAL_MODE        
HardwareSerial RosSerial;
#endif

geometry_msgs::Vector3 accel, gyro, mag;
extern double required_angular_vel;
extern double required_linear_vel_x;
extern double required_linear_vel_y;
extern riki_msgs::Battery raw_battery_msg;
extern riki_msgs::Velocities raw_vel_msg;
extern riki_msgs::Imu raw_imu_msg;
extern riki_msgs::Sonar raw_sonar_msg;
extern uint32_t previous_command_time;




uint16_t check_sum(byte *data)
{
    uint16_t checksum = 0x00;
    for(int i = 4; i < sizeof(RikibotFeedback); i++){
        checksum ^= data[i];
    }
    return checksum;
}

void RikiRosReceiveData()
{
#if SERIAL_MODE  
    if (RosSerial.available()) {
        incomingByte 	  = RosSerial.read();                                   // Read the incoming byte
        bufStartFrame	= ((uint16_t)(incomingByte) << 8) | incomingBytePrev;       // Construct the start frame
    }else {
        return;
    }

    #ifdef SERIAL_DEBUG
        //DebugSerial.print(incomingByte);
        return;
    #endif

    if (bufStartFrame == START_FRAME) {	                    // Initialize if new data is detected
        p       = (byte *)&NewFeedback;
        *p++    = incomingBytePrev;
        *p++    = incomingByte;
        idx     = 2;	
    } else if (idx >= 2 && idx < sizeof(RikibotFeedback)) {  // Save the new received data
        *p++    = incomingByte; 
        idx++;
    }

    if (idx == sizeof(RikibotFeedback)) {
        uint16_t checksum;
        checksum = check_sum((byte *)&NewFeedback);
        // Check validity of the new data
        if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) {
            // Copy the new data
            memcpy(&Feedback, &NewFeedback, sizeof(RosNodeFeedback));
            required_linear_vel_x = Feedback.vel_x;
            required_linear_vel_y = Feedback.vel_y;
            required_angular_vel = Feedback.angular_z;
					  previous_command_time = millis();
            // Print data to built-in Serial
            //Serial3.print("1: ");   Serial3.print(Feedback.vel_x);
            //Serial3.print(" 2: ");  Serial3.print(Feedback.vel_y);
            //Serial3.print(" 3: ");  Serial3.print(Feedback.angular_z);
        } else {
          //Serial3.println("Non-valid data skipped");
        }
        idx = 0;    // Reset the index (it prevents to enter in this if condition in the next cycle)
    }

    // Update previous states
    incomingBytePrev = incomingByte;	
#endif
}

void RikiRosSendData()
{
#if SERIAL_MODE 
    Command.start    = (uint16_t)START_FRAME;
//    Command.checksum = 0;
	  //RosSerial.print("hello world \r\n");
	  //RosSerial.print("baterry is: %f\r\n", raw_battery_msg.battery);

    //sys volt
    Command.volt = raw_battery_msg.battery;
	  //sonar
	
		Command.sonar = raw_sonar_msg.distance;
    //diff vel 
    Command.raw_vel.x = raw_vel_msg.linear_x;
    Command.raw_vel.y = raw_vel_msg.linear_y;
    Command.raw_vel.z = raw_vel_msg.angular_z;

    //imu data
	  Command.mpu_gyro.x = raw_imu_msg.angular_velocity.x;
    Command.mpu_gyro.y = raw_imu_msg.angular_velocity.y;
    Command.mpu_gyro.z = raw_imu_msg.angular_velocity.z;
	
    Command.mpu_accel.x = raw_imu_msg.linear_acceleration.x;
    Command.mpu_accel.y = raw_imu_msg.linear_acceleration.y;
    Command.mpu_accel.z = raw_imu_msg.linear_acceleration.z;


//    sdata = (byte *)&Command;
    for(int i = 4; i < sizeof(Command); i++){
        Command.checksum ^= sdata[i]; 
    }

    RosSerial.write((uint8_t *) &Command, sizeof(Command)); 
#endif
}